﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows.Media.Media3D;

namespace WokseleNaDrzewie
{
    class NonConvex
    {
        public static bool isInside(Rect3D rect, List<Triangle> triangles)
        {
            //sprawdzamy czy wierzchołki sześcianu są wewnątrz modelu
            foreach (Triangle t in triangles)
            {
                double D = -t.normal.X * t.center.X - t.normal.Y * t.center.Y - t.normal.Z * t.center.Z;

                Point3D[] points = new Point3D[8];
                points[0] = new Point3D(rect.Location.X, rect.Location.Y, rect.Location.Z);
                points[1] = new Point3D(rect.Location.X + rect.SizeX, rect.Location.Y, rect.Location.Z);
                points[2] = new Point3D(rect.Location.X + rect.SizeX, rect.Location.Y, rect.Location.Z + rect.SizeZ);
                points[3] = new Point3D(rect.Location.X, rect.Location.Y, rect.Location.Z + rect.SizeZ);
                points[4] = new Point3D(rect.Location.X, rect.Location.Y + rect.SizeY, rect.Location.Z);
                points[5] = new Point3D(rect.Location.X + rect.SizeX, rect.Location.Y + rect.SizeY, rect.Location.Z);
                points[6] = new Point3D(rect.Location.X + rect.SizeX, rect.Location.Y + rect.SizeY, rect.Location.Z + rect.SizeZ);
                points[7] = new Point3D(rect.Location.X, rect.Location.Y + rect.SizeY, rect.Location.Z + rect.SizeZ);

                Point3D[,] edges = new Point3D[12, 2];
                //przednia sciana
                edges[0, 0] = points[0];
                edges[0, 1] = points[1];
                edges[1, 0] = points[1];
                edges[1, 1] = points[2];
                edges[2, 0] = points[2];
                edges[2, 1] = points[3];
                edges[3, 0] = points[3];
                edges[3, 1] = points[0];
                //srodek
                edges[4, 0] = points[0];
                edges[4, 1] = points[4];
                edges[5, 0] = points[1];
                edges[5, 1] = points[5];
                edges[6, 0] = points[2];
                edges[6, 1] = points[6];
                edges[7, 0] = points[3];
                edges[7, 1] = points[7];
                //tylna sciana
                edges[8, 0] = points[4];
                edges[8, 1] = points[5];
                edges[9, 0] = points[5];
                edges[9, 1] = points[6];
                edges[10, 0] = points[6];
                edges[10, 1] = points[7];
                edges[11, 0] = points[7];
                edges[11, 1] = points[4];

                Vector3D[] vectors = new Vector3D[12];
                for (int i = 0; i < 12; i++)
                    vectors[i] = new Vector3D(edges[i, 1].X - edges[i, 0].X, edges[i, 1].Y - edges[i, 0].Y, edges[i, 1].Z - edges[i, 0].Z);

                //płaszczyzna prostopadła do pierwszej krawędzi trójkąta
                Vector3D SN1 = Vector3D.CrossProduct(new Vector3D(t.points[1].X - t.points[0].X, t.points[1].Y - t.points[0].Y, t.points[1].Z - t.points[0].Z), t.normal);
                SN1.Normalize();
                double SD1 = -Vector3D.DotProduct(new Vector3D(t.points[0].X, t.points[0].Y, t.points[0].Z), SN1);

                //płaszczyzna prostopadła do drugiej krawędzi trójkąta
                Vector3D SN2 = Vector3D.CrossProduct(new Vector3D(t.points[2].X - t.points[1].X, t.points[2].Y - t.points[1].Y, t.points[2].Z - t.points[1].Z), t.normal);
                SN2.Normalize();
                double SD2 = -Vector3D.DotProduct(new Vector3D(t.points[2].X, t.points[2].Y, t.points[2].Z), SN2);

                //płaszczyzna prostopadła do trzeciej krawędzi trójkąta
                Vector3D SN3 = Vector3D.CrossProduct(new Vector3D(t.points[0].X - t.points[1].X, t.points[0].Y - t.points[1].Y, t.points[0].Z - t.points[1].Z), t.normal);
                SN2.Normalize();
                double SD3 = -Vector3D.DotProduct(new Vector3D(t.points[0].X, t.points[0].Y, t.points[0].Z), SN3);

                Vector3D V, A, B, P;
                double W, s1, s2, s3;
                for (int i = 0; i < 12; i++)
                {
                    A = new Vector3D(edges[i, 0].X, edges[i, 0].Y, edges[i, 0].Z);
                    B = new Vector3D(edges[i, 1].X, edges[i, 1].Y, edges[i, 1].Z);
                    if ((Vector3D.DotProduct(t.normal, A) + D) * (Vector3D.DotProduct(t.normal, B) + D) >= 0)
                        return false;
                    V = A - B;
                    W = (Vector3D.DotProduct(t.normal, B) + D) / Vector3D.DotProduct(t.normal, V);
                    P = B - V * W;
                    s1 = Vector3D.DotProduct(SN1, P) + SD1;
                    s2 = Vector3D.DotProduct(SN2, P) + SD2;
                    s3 = Vector3D.DotProduct(SN3, P) + SD3;
                    if (Math.Sign(s1) == Math.Sign(s2) && Math.Sign(s2) == Math.Sign(s3))
                        return false;
                }
            }
            return true;
        }
    }
}
